#include "BMI088driver.h"
#include "BMI088reg.h"
#include "bsp_delay.h"

#include "stdint.h"
#include "cmsis_os.h"

#include "main.h"

// float BMI088_ACCEL_SEN = BMI088_ACCEL_3G_SEN;
float BMI088_GYRO_SEN = BMI088_GYRO_2000_SEN;
float BMI088_ACCEL_SEN = BMI088_ACCEL_24G_SEN;


extern SPI_HandleTypeDef hspi1;

// 以寄存器自检对照表
static uint8_t write_BMI088_accel_reg_data_error[BMI088_WRITE_ACCEL_REG_NUM][3] =
        {
                {BMI088_ACC_PWR_CTRL, BMI088_ACC_ENABLE_ACC_ON,       BMI088_ACC_PWR_CTRL_ERROR},
                {BMI088_ACC_PWR_CONF, BMI088_ACC_PWR_ACTIVE_MODE,     BMI088_ACC_PWR_CONF_ERROR},
                {BMI088_ACC_CONF,     BMI088_ACC_NORMAL | BMI088_ACC_1600_HZ |
                                      BMI088_ACC_CONF_MUST_Set, BMI088_ACC_CONF_ERROR},
                {BMI088_ACC_RANGE,    BMI088_ACC_RANGE_24G,           BMI088_ACC_RANGE_ERROR},
                {BMI088_INT1_IO_CTRL, BMI088_ACC_INT1_IO_ENABLE | BMI088_ACC_INT1_GPIO_PP |
                                      BMI088_ACC_INT1_GPIO_LOW, BMI088_INT1_IO_CTRL_ERROR},
                {BMI088_INT_MAP_DATA, BMI088_ACC_INT1_DRDY_INTERRUPT, BMI088_INT_MAP_DATA_ERROR}

        };

static uint8_t write_BMI088_gyro_reg_data_error[BMI088_WRITE_GYRO_REG_NUM][3] =
        {
                {BMI088_GYRO_RANGE,             BMI088_GYRO_2000,         BMI088_GYRO_RANGE_ERROR},
                {BMI088_GYRO_BANDWIDTH,         BMI088_GYRO_1000_116_HZ |
                                                BMI088_GYRO_BANDWIDTH_MUST_Set, BMI088_GYRO_BANDWIDTH_ERROR},
                {BMI088_GYRO_LPM1,              BMI088_GYRO_NORMAL_MODE,  BMI088_GYRO_LPM1_ERROR},
                {BMI088_GYRO_CTRL,              BMI088_DRDY_ON,           BMI088_GYRO_CTRL_ERROR},
                {BMI088_GYRO_INT3_INT4_IO_CONF, BMI088_GYRO_INT3_GPIO_PP |
                                                BMI088_GYRO_INT3_GPIO_LOW,      BMI088_GYRO_INT3_INT4_IO_CONF_ERROR},
                {BMI088_GYRO_INT3_INT4_IO_MAP,  BMI088_GYRO_DRDY_IO_INT3, BMI088_GYRO_INT3_INT4_IO_MAP_ERROR}

        };

#if defined(BMI088_USE_SPI)

#define BMI088_accel_write_single_reg(reg, data) \
    {                                            \
        BMI088_ACCEL_NS_L();                     \
        BMI088_write_single_reg((reg), (data));  \
        BMI088_ACCEL_NS_H();                     \
    }
#define BMI088_accel_read_single_reg(reg, data) \
    {                                           \
        BMI088_ACCEL_NS_L();                    \
        BMI088_read_write_byte((reg) | 0x80);   \
        BMI088_read_write_byte(0x55);           \
        (data) = BMI088_read_write_byte(0x55);  \
        BMI088_ACCEL_NS_H();                    \
    }
#define BMI088_accel_read_muli_reg(reg, data, len) \
    {                                              \
        BMI088_ACCEL_NS_L();                       \
        BMI088_read_write_byte((reg) | 0x80);      \
        BMI088_read_muli_reg(reg, data, len);      \
        BMI088_ACCEL_NS_H();                       \
    }

#define BMI088_gyro_write_single_reg(reg, data) \
    {                                           \
        BMI088_GYRO_NS_L();                     \
        BMI088_write_single_reg((reg), (data)); \
        BMI088_GYRO_NS_H();                     \
    }
#define BMI088_gyro_read_single_reg(reg, data)  \
    {                                           \
        BMI088_GYRO_NS_L();                     \
        BMI088_read_single_reg((reg), &(data)); \
        BMI088_GYRO_NS_H();                     \
    }
#define BMI088_gyro_read_muli_reg(reg, data, len)   \
    {                                               \
        BMI088_GYRO_NS_L();                         \
        BMI088_read_muli_reg((reg), (data), (len)); \
        BMI088_GYRO_NS_H();                         \
    }


#elif defined(BMI088_USE_IIC)

#endif

uint8_t BMI088_init(void) {
    uint8_t error = BMI088_NO_ERROR;

    error |= bmi088_gyro_init();

    error |= bmi088_accel_init();


    return error;
}

uint8_t bmi088_accel_init(void) {
    volatile uint8_t res = 0;
    uint8_t write_reg_num = 0;

    // check commiunication
    BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
    BMI088_delay_ms(1);
    BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
    BMI088_delay_ms(1);

    // accel software reset
    BMI088_accel_write_single_reg(BMI088_ACC_SOFTRESET, BMI088_ACC_SOFTRESET_VALUE);
    BMI088_delay_ms(80);
    //    BMI088_delay_us(150);

    // check commiunication is normal after reset
    BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
    BMI088_delay_ms(1);

    BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
    BMI088_delay_ms(500);

    // check the "who am I"
    if (res != BMI088_ACC_CHIP_ID_VALUE) {
        return BMI088_NO_SENSOR;
    }

    // set accel sonsor config and check
    for (write_reg_num = 0; write_reg_num < BMI088_WRITE_ACCEL_REG_NUM; write_reg_num++) {

        BMI088_accel_write_single_reg(write_BMI088_accel_reg_data_error[write_reg_num][0],
                                      write_BMI088_accel_reg_data_error[write_reg_num][1]);
        //        BMI088_delay_us(BMI088_COM_WAIT_SENSOR_TIME);
        BMI088_delay_ms(80);

        BMI088_accel_read_single_reg(write_BMI088_accel_reg_data_error[write_reg_num][0], res);
        BMI088_delay_ms(10);

        BMI088_accel_read_single_reg(write_BMI088_accel_reg_data_error[write_reg_num][0], res);
        BMI088_delay_ms(1);

        if (res != write_BMI088_accel_reg_data_error[write_reg_num][1]) {
            return write_BMI088_accel_reg_data_error[write_reg_num][2];
        }
    }
    BMI088_delay_ms(1);
    return BMI088_NO_ERROR;
}

uint8_t bmi088_gyro_init(void) {
    uint8_t write_reg_num = 0;
    uint8_t res = 0;

    //神秘bug 暂时保留 之前的写法上面还有sensor读取
   //    reset the gyro sensor
//    BMI088_gyro_write_single_reg(BMI088_GYRO_SOFTRESET, BMI088_GYRO_SOFTRESET_VALUE);
//    //    BMI088_delay_us(150);
//    BMI088_delay_ms(2000);

    // check commiunication is normal after reset
    BMI088_gyro_read_single_reg(BMI088_GYRO_CHIP_ID, res);
    BMI088_delay_ms(100);
    BMI088_gyro_read_single_reg(BMI088_GYRO_CHIP_ID, res);
    BMI088_delay_ms(1);

    // check the "who am I"
    if (res != BMI088_GYRO_CHIP_ID_VALUE) {
        return BMI088_NO_SENSOR;
    }

    // set gyro sonsor config and check
    for (write_reg_num = 0; write_reg_num < BMI088_WRITE_GYRO_REG_NUM; write_reg_num++) {

        BMI088_gyro_write_single_reg(write_BMI088_gyro_reg_data_error[write_reg_num][0],
                                     write_BMI088_gyro_reg_data_error[write_reg_num][1]);
        BMI088_delay_ms(2000);

        //        BMI088_delay_us(BMI088_COM_WAIT_SENSOR_TIME);

        BMI088_gyro_read_single_reg(write_BMI088_gyro_reg_data_error[write_reg_num][0], res);
        BMI088_delay_ms(1);

        if (res != write_BMI088_gyro_reg_data_error[write_reg_num][1]) {
            return write_BMI088_gyro_reg_data_error[write_reg_num][2];
        }
    }
    BMI088_delay_ms(1);

    return BMI088_NO_ERROR;
}
void BMI088_readaccel(float accel[3]){
    get_BMI088_accel(accel);
    accel[0] *= 9.807f;
    accel[1] *= 9.807f;
    accel[2] *= 9.807f;

}

void BMI088_readgyro(float gyro[3]) {
    uint8_t buf[8] = {0};
    int16_t bmi088_raw_temp;

    BMI088_gyro_read_muli_reg(BMI088_GYRO_CHIP_ID, buf, 8);
    if (buf[0] == BMI088_GYRO_CHIP_ID_VALUE) {
        bmi088_raw_temp = (int16_t) ((buf[3]) << 8) | buf[2];
        // 使用弧度单位
        gyro[0] = bmi088_raw_temp * BMI088_GYRO_SEN;
        bmi088_raw_temp = (int16_t) ((buf[5]) << 8) | buf[4];
        gyro[1] = bmi088_raw_temp * BMI088_GYRO_SEN;
        bmi088_raw_temp = (int16_t) ((buf[7]) << 8) | buf[6];
        gyro[2] = bmi088_raw_temp * BMI088_GYRO_SEN;
    }
}

void get_BMI088_gyro(float gyro[3]) {
    //这是专门针对offset写的读取函数,结构和通常的不同

    uint8_t buf[6] = {0, 0, 0, 0, 0, 0};
    int16_t gyro_raw_temp;

    BMI088_gyro_read_muli_reg(BMI088_GYRO_X_L, buf, 6);

    // 用度/秒单位,减少因为pi带来的浮点误差与FPU消耗
    gyro_raw_temp = (int16_t) ((buf[1]) << 8) | buf[0];
    gyro[0] += gyro_raw_temp * BMI088_GYRO_SEN_angle;
    gyro_raw_temp = (int16_t) ((buf[3]) << 8) | buf[2];
    gyro[1] += gyro_raw_temp * BMI088_GYRO_SEN_angle;
    gyro_raw_temp = (int16_t) ((buf[5]) << 8) | buf[4];
    gyro[2] += gyro_raw_temp * BMI088_GYRO_SEN_angle;


}

void get_BMI088_accel(float accel[3]) {
    uint8_t buf[6] = {0, 0, 0, 0, 0, 0};
    int16_t accel_raw_temp;

    BMI088_accel_read_muli_reg(BMI088_ACCEL_XOUT_L, buf, 6);
    // 不计算g,减少因为不确定性G带来的误差
    accel_raw_temp = (int16_t) ((buf[1]) << 8) | buf[0];
    accel[0] = accel_raw_temp * BMI088_ACCEL_SEN;
    accel_raw_temp = (int16_t) ((buf[3]) << 8) | buf[2];
    accel[1] = accel_raw_temp * BMI088_ACCEL_SEN;
    accel_raw_temp = (int16_t) ((buf[5]) << 8) | buf[4];
    accel[2] = accel_raw_temp * BMI088_ACCEL_SEN;
}

float get_BMI088_temperate(void) {
    uint8_t buf[2];
    float temperate;
    int16_t temperate_raw_temp;

    BMI088_accel_read_muli_reg(BMI088_TEMP_M, buf, 2);

    temperate_raw_temp = (int16_t) ((buf[0] << 3) | (buf[1] >> 5));

    if (temperate_raw_temp > 1023) {
        temperate_raw_temp -= 2048;
    }

    temperate = temperate_raw_temp * BMI088_TEMP_FACTOR + BMI088_TEMP_OFFSET;

    return temperate;
}

uint32_t get_BMI088_sensor_time(void) {
    uint32_t sensor_time = 0;
    uint8_t buf[3];
    BMI088_accel_read_muli_reg(BMI088_SENSORTIME_DATA_L, buf, 3);

    sensor_time = (uint32_t) ((buf[2] << 16) | (buf[1] << 8) | (buf[0]));

    return sensor_time;
}

//! ###############################读写函数本体

#if defined(BMI088_USE_SPI)

void BMI088_write_single_reg(uint8_t reg, uint8_t data) {
    BMI088_read_write_byte(reg);
    BMI088_read_write_byte(data);
}

void BMI088_read_single_reg(uint8_t reg, uint8_t *return_data) {
    BMI088_read_write_byte(reg | 0x80);
    *return_data = BMI088_read_write_byte(0x55);
}

void BMI088_read_muli_reg(uint8_t reg, uint8_t *buf, uint8_t len) {
    BMI088_read_write_byte(reg | 0x80);

    while (len != 0) {

        *buf = BMI088_read_write_byte(0x55);
        buf++;
        len--;
    }
}

void BMI088_ACCEL_NS_L(void) {
    HAL_GPIO_WritePin(CS1_ACCEL_GPIO_Port, CS1_ACCEL_Pin, GPIO_PIN_RESET);
}

void BMI088_ACCEL_NS_H(void) {
    HAL_GPIO_WritePin(CS1_ACCEL_GPIO_Port, CS1_ACCEL_Pin, GPIO_PIN_SET);
}

void BMI088_GYRO_NS_L(void) {
    HAL_GPIO_WritePin(CS1_GYRO_GPIO_Port, CS1_GYRO_Pin, GPIO_PIN_RESET);
}

void BMI088_GYRO_NS_H(void) {
    HAL_GPIO_WritePin(CS1_GYRO_GPIO_Port, CS1_GYRO_Pin, GPIO_PIN_SET);
}

//! 关键读取写入函数,但是原先是采取统一写法,这里也就不拆开来了.
uint8_t BMI088_read_write_byte(uint8_t txdata) {
    uint8_t rx_data;
    HAL_SPI_TransmitReceive(&hspi1, &txdata, &rx_data, 1, 1000);

    return rx_data;
}

#elif defined(BMI088_USE_IIC)

#endif


void BMI088_delay_us(uint16_t us) {

    delay_us(us);
}

void BMI088_delay_ms(uint16_t ms) {

    osDelay(ms);
}
